/***************************************************************************************************************************
* drone_sender.cpp
*
* source Author: Qyp（prometheus）
*Author：WXT
*
* Update Time: 2021.6.28
*
* Introduction:  
* 目前的问题，控制逻辑还需加强：
    1.没有锁定后需要解锁才能重新起飞（因为本仿真目的是为了TELLO的控制服务，TELLO LAND后还可以直接起飞）
    2.飞行MOVE模式会响应起飞（takeoff）以及解锁（idle），需要对进程的执行加以限制
    3.rotors的仿真没有限制，如果直接给位置一个很大的值，即可导致过饱和与超调，此时飞行无法稳定
**********************************************************************/

#include <ros/ros.h>

#include "state_from_rotors.h"
#include "command_to_rotors.h"
#include "drone_control_utils.h"
#include "message_utils.h"
#include "control_common.h"
#define NODE_NAME "px4_sender"

using namespace std;
 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float cur_time;                                             //程序运行时间
float Takeoff_height;                                       //默认起飞高度
float Disarm_height;                                        //自动上锁高度
float Land_speed;                                           //降落速度
int Land_mode;                                              //降落策略选择
//Geigraphical fence 地理围栏
Eigen::Vector2f geo_fence_x;
Eigen::Vector2f geo_fence_y;
Eigen::Vector2f geo_fence_z;

Eigen::Vector3d Takeoff_position;                              // 起飞位置
uav_drone_msgs::DroneState _DroneState;                         //无人机状态量

uav_drone_msgs::ControlCommand Command_Now;                      //无人机当前执行命令
uav_drone_msgs::ControlCommand Command_Last;                     //无人机上一条执行命令

Eigen::Vector3d state_sp(0,0,0);
Eigen::Vector3d state_sp_extra(0,0,0);
double yaw_sp;
double yaw_rate_sp;

uav_drone_msgs::Message message;
uav_drone_msgs::LogMessageControl LogMessage;

//RVIZ显示：期望位置
geometry_msgs::PoseStamped ref_pose_rviz;
float dt = 0;

ros::Publisher rivz_ref_pose_pub;
ros::Publisher message_pub;
ros::Publisher log_message_pub;
string mav_name;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void printf_param();
int check_failsafe();
geometry_msgs::PoseStamped get_rviz_ref_posistion(const uav_drone_msgs::ControlCommand& cmd);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void Command_cb(const uav_drone_msgs::ControlCommand::ConstPtr& msg)
{
    // CommandID必须递增才会被记录(目前不知道会不会增加，先屏蔽该项目，直接写入消息)
    Command_Now = *msg;
    /*
    if( msg->Command_ID  >  Command_Now.Command_ID )
    {
        Command_Now = *msg;
    }else
    {
        pub_message(message_pub, uav_drone_msgs::Message::WARN, NODE_NAME, "Wrong Command ID.");
    }
    */
    // 无人机一旦接受到Disarm指令，则会屏蔽其他指令(暂时没有disarm指令，先不进行屏蔽)
    /*
    if(Command_Last.Mode == uav_drone_msgs::ControlCommand::Disarm)
    {
        Command_Now = Command_Last;
    }
    */
}
void station_command_cb(const uav_drone_msgs::ControlCommand::ConstPtr& msg)
{
    Command_Now = *msg;
    pub_message(message_pub, uav_drone_msgs::Message::NORMAL, NODE_NAME, "Get a command from Prometheus Station.");
    
    // 无人机一旦接受到Disarm指令，则会屏蔽其他指令
    /*
    if(Command_Last.Mode == uav_drone_msgs::ControlCommand::Disarm)
    {
        Command_Now = Command_Last;
    }
    */
}
void drone_state_cb(const uav_drone_msgs::DroneState::ConstPtr& msg)
{
    _DroneState = *msg;

    _DroneState.time_from_start = cur_time;
}
void timerCallback(const ros::TimerEvent& e)
{
    pub_message(message_pub, uav_drone_msgs::Message::NORMAL, NODE_NAME, "Program is running.");
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "px4_sender");
    ros::NodeHandle nh("~");
    nh.param<string>("mav_name", mav_name, "firefly");

    //【订阅】指令
    // 本话题为任务模块生成的控制指令
    ros::Subscriber Command_sub = nh.subscribe<uav_drone_msgs::ControlCommand>("/sylu_drone/control_command", 10, Command_cb);

    //【订阅】指令
    // 本话题为Prometheus地面站发送的控制指令//目前可能没有地面站
    ros::Subscriber station_command_sub = nh.subscribe<uav_drone_msgs::ControlCommand>("/prometheus/control_command_station", 10, station_command_cb);
    
    //【订阅】无人机状态
    // 本话题来自drone_pos_estimator.cpp
    ros::Subscriber drone_state_sub = nh.subscribe<uav_drone_msgs::DroneState>("/"+mav_name+"/drone_state", 10, drone_state_cb);

    //【发布】参考位姿 RVIZ显示用
    rivz_ref_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/"+mav_name+"control/ref_pose_rviz", 10);

    // 【发布】用于地面站显示的提示消息
    message_pub = nh.advertise<uav_drone_msgs::Message>("/"+mav_name+"message/main", 10);

    // 【发布】用于log的消息
    log_message_pub = nh.advertise<uav_drone_msgs::LogMessageControl>("/"+mav_name+"log/control", 10);

    // 10秒定时打印，以确保程序在正确运行
    ros::Timer timer = nh.createTimer(ros::Duration(10.0), timerCallback);
    // 参数读取
    nh.param<float>("Takeoff_height", Takeoff_height, 1.0);
    nh.param<float>("Disarm_height", Disarm_height, 0.15);
    nh.param<float>("Land_speed", Land_speed, 0.2);
    nh.param<int>("Land_mode",Land_mode,0);

    nh.param<float>("geo_fence/x_min", geo_fence_x[0], -100.0);
    nh.param<float>("geo_fence/x_max", geo_fence_x[1], 100.0);
    nh.param<float>("geo_fence/y_min", geo_fence_y[0], -100.0);
    nh.param<float>("geo_fence/y_max", geo_fence_y[1], 100.0);
    nh.param<float>("geo_fence/z_min", geo_fence_z[0], -100.0);
    nh.param<float>("geo_fence/z_max", geo_fence_z[1], 100.0);

    // 设定起飞位置
    Takeoff_position[0] = 0.0;
    Takeoff_position[1] = 0.0;
    Takeoff_position[2] = 0.15;

    // 建议控制频率 ： 10 - 50Hz, 控制频率取决于控制形式，若控制方式为速度或加速度应适当提高频率
    ros::Rate rate(20.0);

    // 用于与mavros通讯的类，通过mavros发送控制指令至飞控【本程序->mavros->飞控】
    command_to_rotors _command_to_rotors;

    printf_param();
    
    // 初始化命令-
    // 默认设置：Idle模式 电机怠速旋转 等待来自上层的控制指令
    Command_Now.Mode                                = uav_drone_msgs::ControlCommand::Idle;
    Command_Now.Command_ID                          = 0;
    Command_Now.Reference_State.Move_mode           = uav_drone_msgs::PositionReference::XYZ_POS;
    Command_Now.Reference_State.Move_frame          = uav_drone_msgs::PositionReference::ENU_FRAME;
    Command_Now.Reference_State.position_ref[0]     = 0;
    Command_Now.Reference_State.position_ref[1]     = 0;
    Command_Now.Reference_State.position_ref[2]     = 0;
    Command_Now.Reference_State.velocity_ref[0]     = 0;
    Command_Now.Reference_State.velocity_ref[1]     = 0;
    Command_Now.Reference_State.velocity_ref[2]     = 0;
    Command_Now.Reference_State.acceleration_ref[0] = 0;
    Command_Now.Reference_State.acceleration_ref[1] = 0;
    Command_Now.Reference_State.acceleration_ref[2] = 0;
    Command_Now.Reference_State.yaw_ref             = 0;


    // 记录启控时间
    ros::Time begin_time = ros::Time::now();
    float last_time = drone_control_utils::get_time_in_sec(begin_time);

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        // 当前时间
        cur_time = drone_control_utils::get_time_in_sec(begin_time);
        dt = cur_time  - last_time;
        dt = constrain_function2(dt, 0.02, 0.1);
        last_time = cur_time;

        // 执行回调函数
        ros::spinOnce();

        // Check for geo fence: If drone is out of the geo fence, it will land now.
        if(check_failsafe() == 1)
        {
            Command_Now.Mode = uav_drone_msgs::ControlCommand::Land;
        }

        switch (Command_Now.Mode)
        {
        // 【Idle】 怠速旋转，默认起飞0.1m，对离地进行仿真。
        case uav_drone_msgs::ControlCommand::Idle:
            _command_to_rotors.idle();
            break;

        // 【Takeoff】 从摆放初始位置原地起飞至指定高度，偏航角也保持当前角度
        //      相较于PX4，rotors的起飞实际就是给定位置点，这里写为0，0，2（即原地起飞2m）
        //     后续在TELLO上实现，只需要发布一个takeoff的指令
        case uav_drone_msgs::ControlCommand::Takeoff:
            _command_to_rotors.takeoff(); 
            break;

        // 【Hold】 悬停。当前位置悬停
        case uav_drone_msgs::ControlCommand::Hold:

            if (Command_Last.Mode != uav_drone_msgs::ControlCommand::Hold)
            {
                Command_Now.Reference_State.Move_mode       = uav_drone_msgs::PositionReference::XYZ_POS;
                Command_Now.Reference_State.Move_frame      = uav_drone_msgs::PositionReference::ENU_FRAME;
                Command_Now.Reference_State.position_ref[0] = _DroneState.position[0];
                Command_Now.Reference_State.position_ref[1] = _DroneState.position[1];
                Command_Now.Reference_State.position_ref[2] = _DroneState.position[2];
                Command_Now.Reference_State.yaw_ref         = _DroneState.attitude[2]; //rad

                state_sp = Eigen::Vector3d(_DroneState.position[0],_DroneState.position[1],_DroneState.position[2]);
                yaw_sp = _DroneState.attitude[2]; //rad
            }
            _command_to_rotors.send_pos_setpoint(state_sp, yaw_sp);
            //_command_to_mavros.loiter(); 可用，但不启用

            break;

        // 【Land】 降落。两种降落方式： 只有加载了参数Land_mode为1时，启用第二种降落方式；默认启用第一种降落方式。
        //  第一种：当前位置原地降落，降落后会自动上锁（暂时没做）
        //  第二种：类似于返航模式，设定一个高度5m，升空/降低至该高度，回到home(x=0,y=0)后降落
        case uav_drone_msgs::ControlCommand::Land:

            if(Land_mode == 0)
            {
                _command_to_rotors.land();
            }else
            {
                state_sp = Eigen::Vector3d(0.0,0.0,5.0);
                yaw_sp = 0;
                _command_to_rotors.send_pos_setpoint(state_sp, yaw_sp);
                _command_to_rotors.land();
            }

            break;

        case uav_drone_msgs::ControlCommand::Move:
            state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
            yaw_sp = Command_Now.Reference_State.yaw_ref;
            _command_to_rotors.send_pos_setpoint(state_sp, yaw_sp);
        /*
                            
            if(Command_Now.Reference_State.Move_frame  == uav_drone_msgs::PositionReference::ENU_FRAME)
            {
                if( Command_Now.Reference_State.Move_mode  == uav_drone_msgs::PositionReference::XYZ_POS )
                {
                    state_sp = Eigen::Vector3d(Command_Now.Reference_State.position_ref[0],Command_Now.Reference_State.position_ref[1],Command_Now.Reference_State.position_ref[2]);
                    yaw_sp = Command_Now.Reference_State.yaw_ref;
                }
            }else
            {
                //如果不是ENU坐标系（那就是机体系，rotors不知道是否有机体系）机体系需要考虑坐标转换问题
                pub_message(message_pub, uav_drone_msgs::Message::WARN, NODE_NAME, "Not have to Body contorl");
            }
            //移动模式为XYZ_POS，执行发送命令
            if( Command_Now.Reference_State.Move_mode  == uav_drone_msgs::PositionReference::XYZ_POS )
            {
                _command_to_rotors.send_pos_setpoint(state_sp, yaw_sp);
            }else
            {
                pub_message(message_pub, uav_drone_msgs::Message::WARN, NODE_NAME, "Not Defined. Hold there");
                //rotors没有hold悬停模式，所以用setpoint来代替（本意是用上一次设定的位置XYZ与偏航YAW来实现hold）
                _command_to_rotors.send_pos_setpoint(state_sp, yaw_sp);
                //_command_to_rotors.loiter();
            }
            */
            break;


        // 【Disarm】 上锁
        //      rotors中没有上锁选项，但可以给电机发送command指令，令电机停止，目前未测试过
        //      tello
        case uav_drone_msgs::ControlCommand::Disarm:
            ROS_INFO("NOW，Disarm is not support");
            break;

        // 【User_Mode1】 暂空。可进行自定义
        case uav_drone_msgs::ControlCommand::User_Mode1:
            ROS_INFO("NOW，User_Mode1 is not support，you can do it");
            break;

        // 【User_Mode2】 暂空。可进行自定义
        case uav_drone_msgs::ControlCommand::User_Mode2:
            ROS_INFO("NOW，User_Mode2 is not support，you can do it");
            break;
        }

        //发布用于RVIZ显示的位姿
        ref_pose_rviz = get_rviz_ref_posistion(Command_Now);   
        rivz_ref_pose_pub.publish(ref_pose_rviz);

        //发布log消息，可用rosbag记录
        LogMessage.control_type = PX4_SENDER;
        LogMessage.time = cur_time;
        LogMessage.Drone_State = _DroneState;
        LogMessage.Control_Command = Command_Now;
        LogMessage.ref_pose = ref_pose_rviz;

        log_message_pub.publish(LogMessage);

        Command_Last = Command_Now;
        rate.sleep();
    }

    return 0;

}


void printf_param()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>> px4_sender Parameter <<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "Takeoff_height   : "<< Takeoff_height<<" [m] "<<endl;
    cout << "Disarm_height    : "<< Disarm_height <<" [m] "<<endl;
    cout << "Land_speed       : "<< Land_speed <<" [m/s] "<<endl;
    cout << "Land_mode        : "<< Land_mode << endl;
    cout << "geo_fence_x : "<< geo_fence_x[0] << " [m]  to  "<<geo_fence_x[1] << " [m]"<< endl;
    cout << "geo_fence_y : "<< geo_fence_y[0] << " [m]  to  "<<geo_fence_y[1] << " [m]"<< endl;
    cout << "geo_fence_z : "<< geo_fence_z[0] << " [m]  to  "<<geo_fence_z[1] << " [m]"<< endl;
}

int check_failsafe()
{
    if (_DroneState.position[0] < geo_fence_x[0] || _DroneState.position[0] > geo_fence_x[1] ||
        _DroneState.position[1] < geo_fence_y[0] || _DroneState.position[1] > geo_fence_y[1] ||
        _DroneState.position[2] < geo_fence_z[0] || _DroneState.position[2] > geo_fence_z[1])
    {
        pub_message(message_pub, uav_drone_msgs::Message::ERROR, NODE_NAME, "Out of the geo fence, the drone is landing...");
        return 1;
    }
    else{
        return 0;
    }
}

geometry_msgs::PoseStamped get_rviz_ref_posistion(const uav_drone_msgs::ControlCommand& cmd)
{
    geometry_msgs::PoseStamped ref_pose;

    ref_pose.header.stamp = ros::Time::now();
    // world: 世界系,即gazebo坐标系,参见tf_transform.launch
    ref_pose.header.frame_id = "world";

    if(cmd.Mode == uav_drone_msgs::ControlCommand::Idle)
    {
        ref_pose.pose.position.x = _DroneState.position[0];
        ref_pose.pose.position.y = _DroneState.position[1];
        ref_pose.pose.position.z = _DroneState.position[2];
        ref_pose.pose.orientation = _DroneState.attitude_q;
    }else if(cmd.Mode == uav_drone_msgs::ControlCommand::Takeoff || cmd.Mode == uav_drone_msgs::ControlCommand::Hold)
    {
        ref_pose.pose.position.x = cmd.Reference_State.position_ref[0];
        ref_pose.pose.position.y = cmd.Reference_State.position_ref[1];
        ref_pose.pose.position.z = cmd.Reference_State.position_ref[2];
        ref_pose.pose.orientation = _DroneState.attitude_q;
    }else if(cmd.Mode == uav_drone_msgs::ControlCommand::Disarm  || cmd.Mode == uav_drone_msgs::ControlCommand::Land )
    {
        ref_pose.pose.position.x = cmd.Reference_State.position_ref[0];
        ref_pose.pose.position.y = cmd.Reference_State.position_ref[1];
        ref_pose.pose.position.z = 0.0;
        ref_pose.pose.orientation = _DroneState.attitude_q;
    }
    else if(cmd.Mode == uav_drone_msgs::ControlCommand::Move)
    {
        if( cmd.Reference_State.Move_mode  == uav_drone_msgs::PositionReference::XYZ_POS )
        {
            ref_pose.pose.position.x = cmd.Reference_State.position_ref[0];
            ref_pose.pose.position.y = cmd.Reference_State.position_ref[1];
            ref_pose.pose.position.z = cmd.Reference_State.position_ref[2];
        }else if( Command_Now.Reference_State.Move_mode  == uav_drone_msgs::PositionReference::XYZ_VEL )
        {
            ref_pose.pose.position.x = _DroneState.position[0] + cmd.Reference_State.velocity_ref[0] * dt;
            ref_pose.pose.position.y = _DroneState.position[1] + cmd.Reference_State.velocity_ref[1] * dt;
            ref_pose.pose.position.z = _DroneState.position[2] + cmd.Reference_State.velocity_ref[2] * dt;
        }else if( Command_Now.Reference_State.Move_mode  == uav_drone_msgs::PositionReference::XY_VEL_Z_POS )
        {
            ref_pose.pose.position.x = _DroneState.position[0] + cmd.Reference_State.velocity_ref[0] * dt;
            ref_pose.pose.position.y = _DroneState.position[1] + cmd.Reference_State.velocity_ref[1] * dt;
            ref_pose.pose.position.z = cmd.Reference_State.position_ref[2];
        }else if( Command_Now.Reference_State.Move_mode  == uav_drone_msgs::PositionReference::XY_POS_Z_VEL )
        {
            ref_pose.pose.position.x = cmd.Reference_State.position_ref[0];
            ref_pose.pose.position.y = cmd.Reference_State.position_ref[1];
            ref_pose.pose.position.z = _DroneState.position[2] + cmd.Reference_State.velocity_ref[2] * dt;
        }else if ( Command_Now.Reference_State.Move_mode  == uav_drone_msgs::PositionReference::XYZ_ACC )
        {
            ref_pose.pose.position.x = _DroneState.position[0] + 0.5 * cmd.Reference_State.acceleration_ref[0] * dt * dt;
            ref_pose.pose.position.y = _DroneState.position[1] + 0.5 * cmd.Reference_State.acceleration_ref[1] * dt * dt;
            ref_pose.pose.position.z = _DroneState.position[2] + 0.5 * cmd.Reference_State.acceleration_ref[2] * dt * dt;
        }

        ref_pose.pose.orientation = _DroneState.attitude_q;
    }else
    {
        ref_pose.pose.position.x = 0.0;
        ref_pose.pose.position.y = 0.0;
        ref_pose.pose.position.z = 0.0;
        ref_pose.pose.orientation = _DroneState.attitude_q;
    }

    return ref_pose;
}
